#include "mission_1_fsm.h"
#include "timeout.h"
#include "pid_app.h"
#include "user_led.h"
#include "user_beep.h"
#include "./OLED/OLED.h"
#include "./imu/imu_math.h"
#include "./imu/imu.h"
#include "./lw_pid/lw_pid.h"
#include "./lw_timer/lw_timer_callback.h"

/********************全局变量定义********************/
lwq_t mission_1_fsm_que; /*任务1状态机的消息队列*/
state_machine_t mission_1_fsm; /*任务1状态机*/

/********************静态变量定义********************/
static float marking_angle; /*标记的偏航角度*/
static imu_euler car_euler_angle;

/********************静态函数声明********************/
static void mission_1_idle_handle(state_machine_t* me, param_event_t* e);
static void mission_1_ready_handle(state_machine_t* me, param_event_t* e);
static void walk_straight_line_handle(state_machine_t* me, param_event_t* e);

/********************全局函数定义********************/

/**
 * @brief	任务1状态机初始化
 */
void mission_1_fsm_init(void)
{
	lwq_create(&mission_1_fsm_que, QUE_MISSION_1_MSG_LEN, sizeof(param_event_t)); /*动态创建任务1消息队列*/
	mission_1_fsm.state = MISSION_1_IDLE; /*设置初始状态*/
}

/**
 * @brief	任务1状态机调度函数
 * @param	me 状态机变量
 * @param	event 事件变量
 */
void mission_1_fsm_dispatch(state_machine_t* me, param_event_t* e)
{
	switch(me->state)
	{
		case MISSION_1_IDLE:
			mission_1_idle_handle(me, e);
			break;
		
		case MISSION_1_READY:
			mission_1_ready_handle(me, e);
			break;
		
		case M1_WALK_STRAIGHT_LINE:
			walk_straight_line_handle(me, e);
			break;
	}
}

/********************静态函数定义********************/

/*空闲状态事件处理*/
static void mission_1_idle_handle(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case MISSION_1_ENTER:
		{			
			timeout_event_load(&mission_1_fsm_que, 800, STARTING_DELAY_TIMEOUT); /*挂载超时事件*/
			me->state = MISSION_1_READY; /*切换至就绪状态*/
			break;
		}
		
		case MISSION_1_EXIT:
			break;
	}
}

/*就绪状态事件处理*/
static void mission_1_ready_handle(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case STARTING_DELAY_TIMEOUT:
		{
			imu_get_data(&car_euler_angle, false); /*读取欧拉角*/
			marking_angle = car_euler_angle.yaw; /*获取当前偏航角*/
			pid_set_target(&angle_pid, marking_angle); /*设置角度环目标*/
			pid_output_offset_disable(&angle_pid); /*取消角度环的输出偏移*/
			pid_set_target(&r_speed_pid, 50.0f); /*设置目标速度*/
			pid_set_target(&l_speed_pid, 50.0f);
			pid_enable(&angle_pid); /*使能角度环*/
			gray_twostate_scan_tim_start(&mission_1_fsm_que); /*开启双态灰度扫描*/
			me->state = M1_WALK_STRAIGHT_LINE; /*切换至走直线状态*/
			break;
		}			
	}
}

/*走直线状态事件处理*/
static void walk_straight_line_handle(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case GRAY_SCALE_BLACK:
		{
			led_set();
			beep_set();
			sound_light_tim_start(); /*执行声光提示*/
			pid_output_offset_enable(&angle_pid); /*启用角度环的输出偏移*/
			pid_set_target(&r_speed_pid, 0.0f); /*设置目标速度*/
			pid_set_target(&l_speed_pid, 0.0f);
			pid_disable(&angle_pid); /*失能角度环*/			
			gray_scan_tim_stop(); /*关闭灰度扫描*/
			OLED_ShowString(24, 16, "end of run", OLED_8X16);
			me->state = MISSION_1_IDLE; /*切换至空闲状态*/
			break;
		}			
	}
}